/*
 * Copyright 2025 The Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 *
 *@file    : triangulate.cc
 *@brief   : triangulate
 *@authors : zhanglei
 *@version : v1.0
 *@data    : 2025/9/6
 *
 */

#ifndef TRIANGULATE_H
#define TRIANGULATE_H

#include "common/type.h"
#include "feature/feature_database.h"
#include "optimizer/triangulate_factor.h"
#include "frame/landmark.h"

namespace triangulator {

using namespace feature;
using namespace optimizer;

class Triangulate {
public:
    /**
     * @brief Construct a new triangulate object
     *
     */
    Triangulate(const std::shared_ptr<FeatureDatabase> feature_database_ptr, const Eigen::Matrix3d& intrinsic_matrix, const Eigen::Matrix4d& extrinsic_matrix, const uint8_t& camera_id);
    
    /**
     * @brief Destructor
     *
     */
    ~Triangulate() = default; 

    /**
     * @brief triangulate a track
     * @param[in] cameras camera pose of all observation 
     * @param[in] points normalized points
     * @return point 3d
     */
    const std::vector<Vector3d> TriangulateDLT(const std::map<uint64_t, Eigen::Matrix4d>& poses);

    /**
     * @brief triangulate a track
     * @param[in] cameras camera pose of all observation 
     * @param[in] points normalized points
     * @return point 3d
     */
    const Vector3d TriangulateDLTMethod(const std::vector<Matrix4d>& pose, const std::vector<Vector2d>& normalized_points);
private:
    bool CheckInlier(const Vector3d& world_point, const std::vector<Matrix4d>& pose, const std::vector<Vector2d>& normalized_points, const std::vector<Vector3d>& normalized_point3ds);
    Matrix4d TransformTwb2Tcw(const Matrix4d& Twb, const Matrix4d& Tbc);
    
    Eigen::Vector2d NormalizedToImagePoint(const Eigen::Vector2d& normalized_pt);

    const std::shared_ptr<FeatureDatabase> feature_database_ptr_;
    const Eigen::Matrix3d intrinsic_matrix_; 
    const Eigen::Matrix4d extrinsic_matrix_;
    const uint8_t camera_id_; 

    std::shared_ptr<TriangulateFactor> triangulate_factor_ptr_;

};

}
#endif // TRIANGULATE_H